/**
 *=============================================================================
 *  @copyright (C)      2025CCUT_1024lab_Damocles
 *
 *  @file    template.c
 *  @brief   封装模块代码规范
 *
 *  @note    使用方法
 *
 *  @attention 注意事项
 *-----------------------------------------------------------------------------
 *  @version    @author    @date    @note
 *  V1.0.0      Cherno    250808    done
 *
 *==============================================================================
 */

/*==============================Includes======================================*/
#include "alg_math.h"
#include "alg_pid.h"
/*==============================PrivateMacros=================================*/

/*==============================PrivateTypes==================================*/

/*==============================PrivateVariables==============================*/

/*==============================PrivateFunctionDeclarations===================*/

/*==============================FunctionPrototypes============================*/

/**
 * @brief PID初始化
 *
 * @param _kp P值
 * @param _ki I值
 * @param _kd D值
 * @param _kf 前馈
 * @param _i_out_max 积分限幅
 * @param _out_max 输出限幅
 * @param _dt 时间片长度
 */
void PID::init(float _kp, float _ki, float _kd, float _kf, float _i_out_max,
               float _out_max, float _d_t, float _dead_zone,
               float _i_variable_speed_A, float _i_variable_speed_B,
               float _i_separate_threshold, d_first_e _d_first)
{
	kp = _kp;
	ki = _ki;
	kd = _kd;
	kf = _kf;
	i_out_max = _i_out_max;
	out_max = _out_max;
	d_t = _d_t;
	dead_zone = _dead_zone;
	i_variable_speed_A = _i_variable_speed_A;
	i_variable_speed_B = _i_variable_speed_B;
	i_separate_threshold = _i_separate_threshold;
	d_first = _d_first;
}

/**
 * @brief PID计算值
 */
void PID::calc()
{
	float p_out = 0.0f;  // 比例项输出
	float i_out = 0.0f;  // 积分项输出
	float d_out = 0.0f;  // 微分项输出
	float f_out = 0.0f;  // 前馈输出

	float error = 0.0f;        // 当前误差
	float abs_error = 0.0f;    // 误差绝对值
	float speed_ratio = 0.0f;  // 线性变速积分

	error = target - ref;
	abs_error=math_Abs(error);

	// 判断死区
	if (abs_error < dead_zone) {
		target = ref;
		error = 0.0f;
		abs_error = 0.0f;
	}

	// 计算p项
	p_out = kp * error;

	// 计算i项
	if (i_variable_speed_A == 0.0f && i_variable_speed_B == 0.0f) {
		// 非变速积分
		speed_ratio = 1.0f;
	} else {
		// 变速积分
		if (abs_error <= i_variable_speed_A) {
			speed_ratio = 1.0f;
		} else if (i_variable_speed_A < abs_error &&
		           abs_error < i_variable_speed_B) {
			speed_ratio = (i_variable_speed_B - abs_error) /
			              (i_variable_speed_B - i_variable_speed_A);
		}
		if (abs_error >= i_variable_speed_B) {
			speed_ratio = 0.0f;
		}
	}
	// 积分限幅
	if (i_out_max != 0.0f) {
		math_Limit(&integral_error, -i_out_max / ki, i_out_max / ki);
	}

	if (i_separate_threshold == 0.0f) {
		// 没有积分分离
		integral_error += speed_ratio * d_t * error;
		i_out = ki * integral_error;
	} else {
		// 积分分离使能
		if (abs_error < i_separate_threshold) {
			integral_error += speed_ratio * d_t * error;
			i_out = ki * integral_error;
		} else {
			integral_error = 0.0f;
			i_out = 0.0f;
		}
	}

	// 计算d项
	if (d_first == D_First_DISABLE) {
		// 没有微分先行
		d_out = kd * (error - last_error) / d_t;
	} else {
		// 微分先行使能
		d_out = kd * (out - last_out) / d_t;
	}

	// 计算前馈
	f_out = (target - last_target) * kf;

	// 计算总共的输出
	out = p_out + i_out + d_out + f_out;

	if (out_max != 0.0f) {
		math_Limit(&out, -out_max, out_max);
	}

	// 误差队列：历史误差
	last_out = out;
	last_error = error;
	last_target = target;  // 保存目标值
	last_ref = ref;        // 保存实际值
}
/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
